"""
* 名称：CobotGPT_V5副本_收集轨迹优化数据集.py
* 功能：搭载APF算法，并实现碰撞检测和轨迹优化
* 缺陷：APF算法运行速度缓慢(13s-260s)
"""

import timeit

import numpy as np
import torch
import torch.nn as nn

from Modules.KINECT.User.Releases.kinectcontrol import *
from Modules.AUBO.User.Releases.robotcontrol import *

from Modules.APF.User.Releases.Artifial_potential_field_RRT import APF_RRT, get_obstacle
from Modules.APF.Modules.robot_class import *


class DNN(nn.Module):
    """
    * 功能：碰撞检测模型
    """

    def __init__(self):
        """ 搭建神经网络各层 """
        super(DNN, self).__init__()
        self.net = nn.Sequential(  # 按顺序搭建各层
            nn.Linear(36, 32), nn.Sigmoid(),  # 第 1 层：全连接层
            nn.Linear(32, 8), nn.Sigmoid(),  # 第 2 层：全连接层
            nn.Linear(8, 4), nn.Sigmoid(),  # 第 3 层：全连接层
            nn.Linear(4, 1), nn.Sigmoid()  # 第 4 层：全连接层
        )

    def forward(self, x):
        """ 前向传播 """
        y = self.net(x)  # x 即输入数据
        return y  # y 即输出数据


def delay(microseconds):
    """
    功能：延迟xx微秒，1s = 1*10^3 ms = 1*10^6 us = 1*10^9 ns
    输入：
    输出
    """
    start = timeit.default_timer()
    while True:
        elapsed = (timeit.default_timer() - start) * 10 ** 6
        if elapsed >= microseconds:
            break
    return 0


def Link_Transformation(last_i, i, a_list, alpha_list, d_list, theta_list):
    """
    * function：坐标系{i-1}到坐标系{i}的转换矩阵
    * tips：这里的last_i指的是i-1
    """
    i = i  # 下面使用的i-1表示列表的第i-1个数，注意同DH参数里的i-1区别
    T_martix = np.mat(np.zeros((4, 4)))

    T_martix[0, 0] = np.cos(theta_list[i - 1])
    T_martix[0, 1] = -1 * np.sin(theta_list[i - 1])
    T_martix[0, 2] = 0
    T_martix[0, 3] = a_list[i - 1]

    T_martix[1, 0] = np.sin(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 1] = np.cos(theta_list[i - 1]) * np.cos(alpha_list[i - 1])
    T_martix[1, 2] = -1 * np.sin(alpha_list[i - 1])
    T_martix[1, 3] = -1 * np.sin(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[2, 0] = np.sin(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 1] = np.cos(theta_list[i - 1]) * np.sin(alpha_list[i - 1])
    T_martix[2, 2] = np.cos(alpha_list[i - 1])
    T_martix[2, 3] = np.cos(alpha_list[i - 1]) * d_list[i - 1]

    T_martix[3, 0] = 0
    T_martix[3, 1] = 0
    T_martix[3, 2] = 0
    T_martix[3, 3] = 1

    return T_martix


def get_robot_position(joint_radian: tuple = (0, 0, 0, 0, 0, 0)):
    """
    * 功能：机器人正解，获取六个关节坐标
    * 输入：机械臂关节角
    * 输出：六个关节坐标：6*3=18，类型tuple，单位m
    """
    # 初始化参数（DH参数）
    a_list = [0, 0, 408, 376, 0, 0]
    alpha_list = [0, np.pi / 2, np.pi, np.pi, np.pi / 2, np.pi / 2]
    d_list = [98.5, 121.5, 0, 0, 102.5, 94]
    # theta_list = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 输入想要转动的角度（此处设置转动的角度在转动后即达到机械臂伸直状态）
    theta_offset = [0, np.pi / 2, 0, np.pi / 2, np.pi, 0]  # 关节角补偿
    # print(theta_offset)
    joint_radian = list(joint_radian)
    # print(joint_radian)
    theta_list = [a + b for a, b in zip(joint_radian, theta_offset)]  # 机械臂关节角：正转+，反转-
    # print(theta_list)

    # 例：T_0_1表示坐标系{0}到坐标系{1}的转换矩阵
    T_0_1 = Link_Transformation(0, 1, a_list, alpha_list, d_list, theta_list)
    T_1_2 = Link_Transformation(1, 2, a_list, alpha_list, d_list, theta_list)
    T_2_3 = Link_Transformation(2, 3, a_list, alpha_list, d_list, theta_list)
    T_3_4 = Link_Transformation(3, 4, a_list, alpha_list, d_list, theta_list)
    T_4_5 = Link_Transformation(4, 5, a_list, alpha_list, d_list, theta_list)
    T_5_6 = Link_Transformation(5, 6, a_list, alpha_list, d_list, theta_list)

    T_0_6 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5 * T_5_6  # 二维数组，形状(4,4)，即矩阵
    T_0_5 = T_0_1 * T_1_2 * T_2_3 * T_3_4 * T_4_5
    T_0_4 = T_0_1 * T_1_2 * T_2_3 * T_3_4
    T_0_3 = T_0_1 * T_1_2 * T_2_3
    T_0_2 = T_0_1 * T_1_2
    T_0_1 = T_0_1
    # print(type(T_0_6))    # 查看数组形状：(4,4)
    # T_0_6 = np.array(T_0_6)
    # print(type(T_0_6))

    # 机器人关节坐标
    joint1_pos = T_0_1[0:3, 3].copy()
    joint2_pos = T_0_2[0:3, 3].copy()
    joint3_pos = T_0_3[0:3, 3].copy()
    joint4_pos = T_0_4[0:3, 3].copy()
    joint5_pos = T_0_5[0:3, 3].copy()
    joint6_pos = T_0_6[0:3, 3].copy()
    # print("joint1_pos: ",joint1_pos)
    # print("joint2_pos: ",joint2_pos)
    # print("joint3_pos: ",joint3_pos)
    # print("joint4_pos: ",joint4_pos)
    # print("joint5_pos: ",joint5_pos)
    # print("joint6_pos: ",joint6_pos)

    # <class 'numpy.matrix'> 转换为 <class 'numpy.ndarray'>
    arr1 = np.array(joint1_pos)
    arr2 = np.array(joint2_pos)
    arr3 = np.array(joint3_pos)
    arr4 = np.array(joint4_pos)
    arr5 = np.array(joint5_pos)
    arr6 = np.array(joint6_pos)
    # print(type(arr1))
    # print(arr6)
    # arr1 = arr1.astype(float)
    # arr2 = arr2.astype(float)
    # arr3 = arr3.astype(float)
    # arr4 = arr4.astype(float)
    # arr5 = arr5.astype(float)
    # arr6 = arr6.astype(float)

    # 使用flatten()方法将矩阵降级为一维数组
    flat_array_1 = arr1.flatten() / 1000.0
    flat_array_2 = arr2.flatten() / 1000.0
    flat_array_3 = arr3.flatten() / 1000.0
    flat_array_4 = arr4.flatten() / 1000.0
    flat_array_5 = arr5.flatten() / 1000.0
    flat_array_6 = arr6.flatten() / 1000.0
    # print(flat_array_1)
    # print(flat_array_2)
    # print(flat_array_3)
    # print(flat_array_4)
    # print(flat_array_5)
    # print(flat_array_6)

    # 使用tuple()函数将一维数组转换为元组
    # 改为list
    tuple1_arr = list(flat_array_1)
    tuple2_arr = list(flat_array_2)
    tuple3_arr = list(flat_array_3)
    tuple4_arr = list(flat_array_4)
    tuple5_arr = list(flat_array_5)
    tuple6_arr = list(flat_array_6)
    # print(tuple1_arr)
    # print(tuple2_arr)
    # print(tuple3_arr)
    # print(tuple4_arr)
    # print(tuple5_arr)
    # print(tuple6_arr)
    tuple_all = tuple1_arr + tuple2_arr + tuple3_arr + tuple4_arr + tuple5_arr + tuple6_arr
    # print(tuple_all)
    # print(type(tuple_all))

    joint_position = tuple_all
    # print(T_0_6)
    return joint_position


class CobotGPT:

    def __init__(self):
        """
        功能：设定类里面通用的一些属性，静态值，不能放动态的
        输入：
        输出：连接状态码
        """

        # 配置服务器地址和端口
        # self.ip = '169.254.94.55'
        self.ip = '192.168.1.6'  # 一楼：注意配置网线口的适配器属性：关掉IPV6，手动设置IPV4地址和机械臂在同一个网段（README）
        self.port = 8899

        # 预设的机械臂工作路径点：9个
        self.joint_radian_list = [
            (80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
            (160 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
            (160 * pi / 180, -30 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
            (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 0 * pi / 180, 0 * pi / 180, 0 * pi / 180),
            (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 0 * pi / 180, 0 * pi / 180),
            (160 * pi / 180, -30 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),  # 第一阶段
            (160 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
            (90 * pi / 180, 0 * pi / 180, 80 * pi / 180, 20 * pi / 180, 90 * pi / 180, 0 * pi / 180),
            (90 * pi / 180, -20 * pi / 180, 80 * pi / 180, 10 * pi / 180, 90 * pi / 180, 0 * pi / 180), ]

        # 轨迹优化起点，终点
        self.star = None
        self.goal = None

    # def robot_set_position(self, cnt:int, queue1: Queue, queue2: Queue):
    def robot_set_position(self, cnt: int, joint_radian_queue: Queue):
        """
        * 功能：设置机械臂的固定轨迹点，并实时获取关节角信息
        * 输入：cnt输入测试次数，queue1输入是否碰撞(没用到)，queue2输入机械臂关节角
        * 输出：
        """

        # 初始化logger
        logger_init()
        # 启动测试
        # logger.info("Func1: {0} robot_set_position() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        # logger.info("Func1：robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("Func1: {0} robot_set_position() running...".format(Auboi5Robot.get_local_time()))
                # 上电
                robot.robot_startup()
                # 设置碰撞等级
                robot.set_collision_class(10)
                # 初始化全局配置文件
                robot.init_profile()

                # 设置关节最大加速度
                robot.set_joint_maxacc((1.5, 1.5, 1.5, 1.5, 1.5, 1.5))
                # 设置关节最大速度
                # robot.set_joint_maxvelc((0.2, 0.2, 0.2, 0.2, 0.2, 0.2))
                robot.set_joint_maxvelc((0.1, 0.1, 0.1, 0.1, 0.1, 0.1))  # 低速挡
                # robot.set_joint_maxvelc((0.3, 0.3, 0.3, 0.3, 0.3, 0.3))

                # 回到初始位姿
                joint_radian_ori = (0, 0, 0, 0, 0, 0)  # 初始位姿,turple类型
                logger.info("回到初始位姿：{0}".format(joint_radian_ori))
                robot.move_joint(joint_radian_ori)

                # 循环测试
                while cnt > 0:
                    cnt -= 1
                    # i = 0
                    # while i < int(len(self.joint_radian_list)):
                    #     joint_radian = joint_radian_queue.get()
                    #     logger.info("move joint to {0}".format(joint_radian))
                    #     robot.move_joint(joint_radian)
                    #     i += 1
                    joint_radian = joint_radian_queue.get()
                    logger.info("move joint to {0}".format(joint_radian))
                    robot.move_joint(joint_radian)

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in robot_set_position()".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            print("robot disconnected-------------------------")

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} Func1 completed.".format(Auboi5Robot.get_local_time()))

    def dataset(self, skeleton_position_queue: Queue, position_data_queue: Queue):
        """
        功能：将坐标数据转换成数据集
        输入：queue1:输入骨骼坐标，queue2：输出36列一维数据
        输出：
        """

        # 初始化logger
        logger_init()
        # 启动测试
        # logger.info("Func2：{0} dataset() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        # logger.info("Func2: robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("Func2：{0} dataset() running...".format(Auboi5Robot.get_local_time()))
                while result == RobotErrorType.RobotError_SUCC:
                    # 获取当前机械臂六个关节角
                    current_waypoint = robot.get_current_waypoint()
                    data1 = current_waypoint["joint"]

                    # 将此刻skeleton_position_queue管道（骨骼坐标）清空，并获取最新骨骼坐标
                    while skeleton_position_queue.empty() is not True:
                        skeleton_position_queue.get()
                    # delay(1000) # 间隔1ms，防止管道来不及填充数据（说明：不能放延迟，会导致程序一卡一卡的）
                    data2 = skeleton_position_queue.get()

                    # position_data_queue管道（36列一维数据）放入数据（36列：骨骼坐标 + 六个关节角）
                    data = data1 + data2
                    position_data_queue.put(data)

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in dataset()".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            print("robot disconnected-------------------------")

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} Func2 completed.".format(Auboi5Robot.get_local_time()))

    def collision_detection(self, collision_flag_queue: Queue, next_joint_radian_queue: Queue, position_data_queue: Queue):
        """
        功能：碰撞检测
        输入：queue1:输出是否碰撞，queue2:输出下一个关节角，queue3：输入36列一维数据
        输出：
        """

        collision_flag = 0  # 默认未发生碰撞
        step = 0    # 机器人轨迹点标记

        # 加载离线碰撞检测模型(DNN)
        model_collision_detection = torch.load('DNN_model2.pth')  # 加载训练好的碰撞检测模型
        model_collision_detection.to('cuda:0')  # 模型加载到GPU上

        # model_path_optimize = torch.load('DNN_model2.pth')  # 加载训练好的轨迹优化模型
        # model_path_optimize.to('cuda:0')  # 模型加载到GPU上

        # 初始化logger
        logger_init()
        # 启动测试
        # logger.info("{0} collision_detection() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        # logger.info("Func3：robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                logger.info("{0} collision_detection() running...".format(Auboi5Robot.get_local_time()))
                while True:
                    # collision_flag_queue（是否碰撞）、next_joint_radian_queue（下一个关节角）管道战术清空
                    while collision_flag_queue.empty() is not True:
                        collision_flag_queue.get()
                    while next_joint_radian_queue.empty() is not True:
                        next_joint_radian_queue.get()

                    # 运行正常，传输下一个关节角给机械臂
                    if collision_flag == 0:
                        # print("collision 运行正常传输关节角")
                        collision_flag_queue.put(0)
                        next_joint_radian_queue.put(self.joint_radian_list[step])
                        step += 1
                        if step == int(len(self.joint_radian_list)):
                            step = 0
                    # 发生碰撞，传输碰撞信号
                    else:
                        # print("collision 传输碰撞信号")
                        # collision_flag = 0
                        collision_flag_queue.put(1)

                    # 碰撞检测：模型输入数据获取
                    X = position_data_queue.get()  # 获取36列一维数据，碰撞检测

                    # 轨迹优化：模型输入数据获取，组成：机器人关节坐标(6*3=18)，人体骨骼坐标(10*3=30)，当前关节角(6)，目标点关节角(6)
                    joint_radian = X[0:6]  # 6列
                    skeleton_data = X[5:-1]  # 30列
                    robot_position = get_robot_position(joint_radian)  # 18列
                    O = robot_position + skeleton_data  # 获取48列一维数据，轨迹优化

                    # print(f"self.data:{self.data}")
                    # print(len(X))
                    # print(X)

                    # 碰撞检测模块，用模型1
                    if len(X) == 36:
                        arr = np.array(X)  # 列表 => numpy数组
                        arr = arr.astype(np.float32)  # 转为 float32 类型numpy数组
                        ts = torch.tensor(arr)  # numpy数组 => 张量
                        ts = ts.to('cuda')
                        with torch.no_grad():  # 该局部关闭梯度计算功能
                            pred = model_collision_detection(ts)  # 一次前向传播（批量）
                            pred[pred >= 0.5] = 1
                            pred[pred < 0.5] = 0
                            pred = int(pred.item())  # 张量 => int值
                            print(f"是否碰撞: {pred}")
                    else:
                        # print("输入的总坐标格式错误")
                        pred = 0

                    # 碰撞检测结果处理模块
                    if pred == 1:
                        collision_flag = 1
                        robot.move_pause()
                        # 加轨迹优化，用模型2
                        # pass
                        # # 轨迹优化模块，用模型2
                        # if len(O) == 48:
                        #     arr = np.array(X)  # 列表 => numpy数组
                        #     arr = arr.astype(np.float32)  # 转为 float32 类型numpy数组
                        #     ts = torch.tensor(arr)  # numpy数组 => 张量
                        #     ts = ts.to('cuda')
                        #     with torch.no_grad():  # 该局部关闭梯度计算功能
                        #         pred = model_path_optimize(ts)  # 一次前向传播（批量）
                        #         pred[pred >= 0.5] = 1
                        #         pred[pred < 0.5] = 0
                        #         pred = int(pred.item())  # 张量 => int值
                        #         print(f"是否碰撞: {pred}")
                        # else:
                        #     # print("输入的总坐标格式错误")
                        #     pred = 0

                        # 设定机器人的关节角空间
                        joint_limit_angle = [
                            Joint_point(
                                [-170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180, -170 * pi / 180,
                                 -170 * pi / 180]),
                            Joint_point([170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180, 170 * pi / 180,
                                         170 * pi / 180])]

                        # 获取的当前的关节坐标、骨骼坐标信息、当前关节角、目标关节角
                        skeleton_position_list = list(skeleton_data)  # 3*10=30
                        joint_position_list = list(robot_position)  # 3*6=18
                        star = list(joint_radian)
                        goal = [170 * pi / 180, -20 * pi / 180, 80 * pi / 180, 0 * pi / 180, 90 * pi / 180, -170 * pi / 180]
                        self.star = Joint_point(star)
                        self.goal = Joint_point(goal)

                        # 先确定障碍物坐标，即找出距离机器人关节最近的骨骼点坐标，并将骨骼点坐标实例化为实体障碍物
                        cube_obstacle = get_obstacle(skeleton_position_list, joint_position_list)
                        print("obstacle positiopn(xyz,lwh):", cube_obstacle.x, cube_obstacle.y, cube_obstacle.z,
                              cube_obstacle.l, cube_obstacle.w, cube_obstacle.h)

                        # 运行人工势场法
                        apf_rrt = APF_RRT(self.start, self.goal, cube_obstacle, joint_limit_angle)
                        # path = apf_rrt.path_planning(queue2)

                        start = time.time()
                        path = apf_rrt.path_planning()
                        end = time.time()
                        print("轨迹优化完成耗费{:.2f}s".format(end - start))

                        # 运行轨迹优化路径点
                        path_list = list()
                        for i in range(len(path)):
                            path_list.append(path[i].theta)
                        # print("path_list:", path_list)

                        collision_flag = 0
                        robot.move_continue()
                        for i in range(len(path_list)):
                            path = tuple(path_list[i])
                            logger.info("move joint to {0}".format(path))
                            robot.move_joint(path)




        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1} in collision_detection()".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            print("robot disconnected-------------------------")

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} Func3 completed.".format(Auboi5Robot.get_local_time()))

    def collision_path_optimize(self, queue1: Queue):
        """
        * 功能：轨迹优化
        * 输入：queue1:输入骨骼坐标
        * 输出：机械臂下一个运动关节角，类型tuple
        """

        # 初始化logger
        logger_init()
        # 启动测试
        logger.info("Func4：{0} collision_path_optimize() running...".format(Auboi5Robot.get_local_time()))
        # 系统初始化
        Auboi5Robot.initialize()
        # 创建机械臂控制类
        robot = Auboi5Robot()
        # 创建上下文
        handle = robot.create_context()
        # 打印上下文
        logger.info("robot.rshd={0}".format(handle))

        try:
            # 连接服务器
            ip = self.ip
            port = self.port
            result = robot.connect(ip, port)
            if result != RobotErrorType.RobotError_SUCC:
                logger.info("connect server{0}:{1} failed.".format(ip, port))
            else:
                pass

        # 出现错误的处理
        except RobotError as e:
            logger.error("{0} robot Event:{1}".format(robot.get_local_time(), e))

        except KeyboardInterrupt:
            # 断开服务器链接
            if robot.connected:
                # 断开机械臂链接
                robot.disconnect()
            # 释放库资源
            Auboi5Robot.uninitialize()
            print("robot disconnected-------------------------")

        # 无论是否有异常，都会执行的代码
        finally:
            # 断开服务器链接
            # if robot.connected:
            #     # 关闭机械臂
            #     robot.robot_shutdown()
            #     # 断开机械臂链接
            #     robot.disconnect()
            # # 释放库资源
            # Auboi5Robot.uninitialize()
            logger.info("{0} test completed.".format(Auboi5Robot.get_local_time()))


if __name__ == '__main__':
    # 创建类实例
    game = BodyGameRuntime()
    cobot = CobotGPT()

    # 创建通信队列
    q1 = Queue(maxsize = 1)  # q1:是否碰撞，
    q2 = Queue(maxsize = 1)  # q2:机械臂关节角
    q3 = Queue()  # q3:kinect_waypoint
    q4 = Queue(maxsize = 1)  # q4:36列坐标

    # 创建子进程
    p1 = Process(target = cobot.robot_set_position, kwargs = {"cnt": 5, "queue1": q1, "queue2": q2})
    p2 = Process(target = cobot.collision_detection, kwargs = {"queue1": q1, "queue2": q2, "queue3": q4})
    p3 = Process(target = cobot.dataset, kwargs = {"queue1": q3, "queue2": q4})
    p1.start()
    p2.start()
    p3.start()

    # 主进程
    game.run(q3)
